#include "optical.h"
#include "LPC17xx.h"
#include "lpc_types.h"
#include "KeyboardHost.h"
#include <math.h>
#include "Timer.h"
#include "lpc17xx_timer.h"
#define DEBUG 1
#define PI 3.14159265

#if DEBUG

#include "serial.h"
#include "lpc17xx_gpio.h"
#include <stdlib.h>

#endif



int8_t x = 0;
int8_t y = 0;
int32_t relX = 0;
int32_t relY = 0;


#if DEBUG

void set_led(int id, int on)
{
	switch(id)
	{
		case 1: {id = 18;break;}
		case 2: {id = 20;break;}
		case 3: {id = 21;break;}
		case 4: {id = 23;break;}
	}
	if(on)
	{
		GPIO_SetDir(1, (0x1<<id), 1);
		GPIO_SetValue(1, (0x1<<id));
	}
	else GPIO_SetDir(1, (0x1<<id), 0);

}


void displayResult(int32_t value)
{
        uint8_t all[11];
        sprintf(all+1, "%d\n\r", value);
        //all[0] = (value < 0 ? '-' : '+');
        all[0] = '>';
        write_usb_serial_blocking(all, 10);
}

#endif

int32_t getRelativePositionX()
{
    return relX;
}

int32_t getRelativePositionY()
{
    return relY;
}

int8_t getX()
{
    return x;
}

int8_t getY()
{
    return y;
}

int32_t getAngle()
{
    return atan(fabs(y)/fabs(x)) * 180 / PI;   
}

int32_t getTotalAngle()
{
    return atan(fabs(relY)/fabs(relX)) * 180 / PI;       
}


void cb(uint8_t buttons, int8_t X, int8_t Y)
{
    x = X;
    y = Y;
    relX += x;
    relY += y;
#if DEBUG
    set_led(1,1);
    displayResult(relX);
    displayResult(relY);
#endif
}


void attach()
{
#if DEBUG
    set_led(2,1);
#endif
}    

void detach()
{
#if DEBUG
    set_led(2,0);
#endif
}

void TIMER0_IRQHandler()
{
    x = 0;
    y = 0;
    if(TIM_GetIntStatus(LPC_TIM0, TIM_MR0_INT) == SET)
    {
        mouse_poll();
    }
    TIM_ClearIntPending(LPC_TIM0, TIM_MR0_INT);
}

void opticalInit()
{
    x = 0;
    y = 0;
    relX = 0;
    relY = 0;
#if DEBUG
    serial_init();
#endif  
    mouse_init(cb, attach, detach);
    initTimer0(5);
#if DEBUG
    set_led(3,1);
#endif

         
}
